/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "URG.h"
#include "mathutils.h"

#define RANGE 4.0
#define NUMBER_OF_RAYS 240
#define MIN_ANGLE DegToRad(-120)
#define MAX_ANGLE DegToRad(120)

CURG::CURG(CWorld *World, CObject *ParentObject, dBodyID parentBody, 
        dReal *posOffset, dMatrix3 rotOffset, float interval, 
        MaCI::MaCICtrl::CMaCICtrlServer *mcs, int minor, std::string interfaceName)
:CLaserScanner(World,ParentObject,parentBody,posOffset, rotOffset, interval, 
    NUMBER_OF_RAYS, RANGE, MIN_ANGLE, MAX_ANGLE, mcs, minor),
PosOffset(posOffset),
material(DEFAULT_MATERIAL),
box(0.05,0.05,0.041,this,this->SpaceID,parentBody,&material,false)
{
  for (int i=0; i<12;i++)
    this->RotOffset[i]=rotOffset[i];
  
  
  dGeomSetOffsetPosition(box.GeomID,posOffset[0],posOffset[1]-0.04,posOffset[2]);
  dGeomSetOffsetRotation(box.GeomID,rotOffset);

  material.look.color[0]=0.7;
  material.look.color[1]=0.7;
  material.look.color[2]=0.55;
  
  //rayCluster->IsVisible=true;
}
    
void CURG::Draw()
{
  dReal M[16];
    box.Draw();
    glPushAttrib(GL_ALL_ATTRIB_BITS);
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
      box.GetGeomTransMatrix(M);
      glMultMatrixf(M);
      glColor3f(0.2,0,0);
      float mcolor[3] = {0.2, 0.0, 0.0}; 
      glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);

      glTranslatef(0,0.041,0);
      glRotatef(90,1,0,0);
      drawCylinder(0.038/2.0,0.029,PRIMITIVE_DETAIL_LEVEL);

    glPopMatrix();
    glPopAttrib();
    
    rayCluster->Draw();
}
